function [bi, wi] = fitrobustbisquare_img(X, yi, tune, olt)
% fitrobustbisquare_img  - faster fitting function for robust bisquare
%
% FORMAT:  [bi, wi] = fitrobustbisquare_img(X, yi [, tune [, olt]])
%
% Input fields:
%
%       X           model (will NOT be patched, so must contain confounds)
%       yi          data
%       tune        bisquare tuning parameter (default: 4.685)
%       olt         outlier threshold (value from 0...1, default sqrt(N))
%
% Output fields:
%
%       bi          beta weights
%       wi          weighting vector
%
% Note: other than the robustfit function of the stats toolbox, NaNs
%       must be removed *prior* to regression/estimation, manually
%       also, if the data contain more than olt zeros (per case), those
%       will be removed first and the weights then set to zero after
%       regression

% Version:  v0.7g
% Build:    9040222
% Date:     Apr-02 2009, 10:18 PM CEST
% Author:   Jochen Weber, SCAN Unit, Columbia University, NYC, NY, USA
% URL/Info: http://wiki.brainvoyager.com/BVQXtools

% check arguments
if nargin < 2 || ...
   ~isa(X, 'double') || ...
   ~isnumeric(yi) || ...
    isempty(X) || ...
    ndims(X) > 2 || ...
    size(X, 2) > size(X, 1) || ...
    any(isinf(X(:)) | isnan(X(:))) || ...
    size(yi, ndims(yi)) ~= size(X, 1) || ...
    any(isinf(yi(:)) | isnan(yi(:)))
    error( ...
        'BVQXtools:BadArgument', ...
        'Bad or missing argument.' ...
    );
end
if ~isa(yi, 'double')
    yi = double(yi);
end
if nargin < 3 || ...
   ~isa(tune, 'double') || ...
    numel(tune) ~= 1 || ...
    isinf(tune) || ...
    isnan(tune) || ...
    tune <= 0
    tune = 4.685;
end
sy = size(yi);
sy(end) = [];
ny = prod(sy);
[n, p] = size(X);
op = ones(1, p);
if nargin < 4 || ...
   ~isa(olt, 'double') || ...
    numel(olt) ~= 1 || ...
    isinf(olt) || ...
    isnan(olt) || ...
    olt <= 0
    olt = ceil(n - sqrt(n));
elseif olt > 1
    olt = n;
else
    olt = n * olt;
end

% find the least squares solution.
[Q, h, perm] = qr(X, 0);
tol = abs(h(1)) * n * eps(class(h));
xrank = sum(abs(diag(h)) > tol);
if xrank ~= p
    error( ...
        'BVQXtools:RankDeficient', ...
        'X is rank deficient, rank = %d', ...
        xrank ...
    );
end

% preparation values
X = X(:, perm);
[pp, perm] = sort(perm);
Q = sqrt(eps(class(X)));
h = X / h;
h = min(.9999, sum(h .* h, 2));
h = 1 ./ sqrt(1 - h);
strect = struct('RECT', true);

% bring data into better format
yi = reshape(yi, ny, n);

% create output "images"
bi = zeros(ny, p);
if nargout > 1
    wi = ones(ny, n);
end
b = zeros(p, 1);
b0 = b;

% test BVQXprogress
if ny > 500
    try
        pbar = BVQXprogress;
        BVQXprogress(pbar, 'setposition', [80, 200, 640, 36]);
        BVQXprogress(pbar, 'settitle', sprintf('Robustly fitting %d-x-%d matrix...', n, p));
        BVQXprogress(pbar, 0, sprintf('Estimating %d samples...', ny), 'visible', 0, ny);
        vcs = round(max(100, ny / 100));
        vcn = vcs;
    catch
        pbar = [];
        vcn = Inf;
    end
else
    pbar = [];
    vcn = Inf;
end

% iterate over voxels
for vc = 1:ny
    
    % fill beta estimates
    y = yi(vc, :)';
    
    % how many values
    y0 = y ~= 0;
    y0s = sum(y0);
    
    % too few values
    if y0s <= p
        
        % continue immediately
        continue;
        
    % enough values to use full data (hopefully rejecting outliers)
    elseif y0s >= olt
        
        % follow simplified path
        b = linsolve(X, y, strect);
        b0(:) = 0;
        wxrank = xrank;

        % handle case of "almost perfect fit"
        tiny_s = 1e-6 * std(y);
        if tiny_s == 0
            tiny_s = Q;
        end

        % perform iteratively reweighted least squares to get coefficient estimates
        iter = 1;
        while any(abs(b - b0) > Q * max(abs(b), abs(b0))) || iter == 1

            % break if too many iterations
            if (iter > 50)
                break;
            end

            % compute residuals from previous fit, then compute scale estimate
            r = y - X * b;
            radj = r .* h;
            s = madsigma(radj, wxrank);

            % compute new weights from these residuals, then re-fit
            w = radj / (max(s, tiny_s) * tune);
            w = (abs(w) < 1) .* (1 - w .^ 2) .^ 2;
            b0 = b;
            sw = sqrt(w);
            [b, wxrank] = linsolve(X .* sw(:, op), y .* sw, strect);
            iter = iter + 1;
        end
        
        % store values
        if nargout > 1
            wi(vc, :) = w';
        end
        bi(vc, :) = b;

    % not enough data
    else
        
        % get reduced model
        Xr = X(y0, :);
        Xrc = any(Xr ~= 0, 1);
        
        % use external function on partial data
        try
            [bex, rex, wex] = fitrobustbisquare(Xr(:, Xrc), y(y0), tune);
            
        % yet error, bad design matrix -> unestimable
        catch
            continue;
        end
        
        % store values into good portion
        if nargout > 1
            wi(vc, y0) = wex';
            wi(vc, ~y0) = 0;
        end
        bi(vc, Xrc) = bex;
    end

    % update progress bar
    if vc >= vcn && ...
       ~isempty(pbar)
        BVQXprogress(pbar, vc);
        vcn = vcn + vcs;
    end

end

% reshape outputs
bi = reshape(bi(:, perm), [sy, p]);
if nargout > 1
    wi = reshape(wi, [sy, n]);
end

% clear progress bar
if ~isempty(pbar)
    closebar(pbar);
end

% -----------------------------

function s = madsigma(r, p)
%MADSIGMA    Compute sigma estimate using MAD of residuals from 0
rs = sort(abs(r));
ns = numel(rs) + p;
if mod(ns, 2) == 0
    s = rs(ns / 2) / 0.6745;
else
    s = (rs(ns / 2 - 0.5) + rs(ns / 2 + 0.5)) / 1.349;
end
